

/*
 * 文件名：hbot_motion.cpp
 * 功能描述：H-Bot运动控制算法
 */
#include "hbot_motion.h"

float hbot_x = 0.0;
float hbot_y = 0.0;
long hbot_position[2] = {0, 0};

#ifdef HBOT_AXIS_1
// X | +a,0 | 顺，顺
MotorSteps calc_vector_x(float dis)
{
    MotorSteps motor_steps;
    motor_steps.m1_step_value = (long)(dis * HBOT_STEP_DIS);
    motor_steps.m2_step_value = (long)(dis * HBOT_STEP_DIS);
    return motor_steps;
}
// Y | 0,a | 逆，顺
MotorSteps calc_vector_y(float dis)
{
    MotorSteps motor_steps;
    motor_steps.m1_step_value = (-1) * (long)(dis * HBOT_STEP_DIS);
    motor_steps.m2_step_value = (long)(dis * HBOT_STEP_DIS);
    return motor_steps;
}
#elif defined(HBOT_AXIS_2)

// X=Y
MotorSteps calc_vector_x(float dis)
{
    MotorSteps motor_steps;
    motor_steps.m1_step_value = (-1) * (long)(dis * HBOT_STEP_DIS);
    motor_steps.m2_step_value = (long)(dis * HBOT_STEP_DIS);
    return motor_steps;
}
// Y=-X
MotorSteps calc_vector_y(float dis)
{
    MotorSteps motor_steps;
    motor_steps.m1_step_value = (-1) * (long)(dis * HBOT_STEP_DIS);
    motor_steps.m2_step_value = (-1) * (long)(dis * HBOT_STEP_DIS);
    return motor_steps;
}
#endif // HBOT_AXIS